#include "RigidBody.h"
#include "ConvertUtil.h"
#include <string>

using namespace std;

extern string model;

RigidBody::RigidBody(void)
{
	density = 1.0;
	initRigidBody(conversionFromObj);
}

RigidBody::RigidBody( string filename, double density, double res )
{
	this->density = density;
	this->res = res;
	model = filename;
	initRigidBody(conversionFromObj);
}


RigidBody::~RigidBody(void)
{
}

void RigidBody::computeCenterOfMass()
{
	cm = Vec2d(0, 0);
	Vec2d origin(0, 0);
	double area = 0;

	for (int i=0; i<poly->edgeCount; i++)
	{
		Edge e = poly->edgeList[i];

		Vert pa = poly->vertList[e.a];
		Vert pb = poly->vertList[e.b];

		Vec2d va(pa.x, pa.y);
		Vec2d vb(pb.x, pb.y);

		Vec2d oa = va - origin;
		Vec2d ob = vb - origin;

		double temp = abs(dot(Vec2d(-1 * oa[1], oa[0]), ob)) / 2.0;

		area += temp;

		cm += temp * (origin + va + vb) / 3.0;
	}

	mass = area * density;

	cm /= area;
}

void RigidBody::computeMomentOfInertia()
{
	moment = 0;

	for (int i=0; i<poly->edgeCount; i++)
	{
		Edge e = poly->edgeList[i];

		Vert pa = poly->vertList[e.a];
		Vert pb = poly->vertList[e.b];

		Vec2d va(pa.x, pa.y);
		Vec2d vb(pb.x, pb.y);

		Vec2d ma = va - cm;
		Vec2d mb = vb - cm;

		double t = dot(Vec2d(-1 * ma[1], ma[0]), mb) * (dot(ma, ma) + dot(ma, mb) + dot(mb, mb));

		moment += t;
	}

	moment = abs(moment);
	moment /= 12.0;
	moment *= density;
}

void RigidBody::computeMassProperty()
{
	computeCenterOfMass();
	computeMomentOfInertia();
}

void RigidBody::initRigidBody( initializeFunc setUp )
{
	this->pos = Vec2d(0, 0);
	this->vel = Vec2d(0, 0);
	this->orientation = 0;
	this->rotation = 0;

	poly = new Polygon();
	poly->buildPolyStructure(setUp);

	computeMassProperty();

	poly->translatePolygon(-1.0 * cm);

	sdm = new GridSDM(poly, res);

	radius = 0.0;

	for (int i=0; i<poly->vertCount; i++)
	{
		Vec2d vert(poly->vertList[i].x, poly->vertList[i].y);

		radius = max(radius, mag(vert));
	}
}

void RigidBody::rebuildMatrices()
{	
	orientationMatrix = Mat22d(cos(orientation), sin(orientation), -1.0 * sin(orientation), cos(orientation));
	inverseOrientationMatrix = orientationMatrix.transpose();
}

Vec2d RigidBody::getWorldVelocity( Vec2d x )
{
	rebuildMatrices();

	Vec3d Rx = Vec3d(x[0], x[1], 0) - Vec3d(pos[0], pos[1], 0);
	Vec3d rotateZ(0, 0, rotation);
	Vec3d angular = cross(rotateZ, Rx);

	return vel + Vec2d(angular[0], angular[1]);
}

Vec2d RigidBody::getPointWorldToBody( Vec2d p )
{
	rebuildMatrices();

	return inverseOrientationMatrix * (p - pos);
}

Vec2d RigidBody::getPointBodyToWorld( Vec2d p )
{
	rebuildMatrices();

	return orientationMatrix * p + pos;
}

Vec2d RigidBody::getVectorWorldToBody( Vec2d v )
{
	rebuildMatrices();

	return inverseOrientationMatrix * v;
}

Vec2d RigidBody::getVectorBodyToWorld( Vec2d v )
{
	rebuildMatrices();

	return orientationMatrix * v;
}

Value RigidBody::evaluateSDM( Vec2d p, bool compute_normal )
{
	return sdm->getEvaluation(p, compute_normal);
}

void RigidBody::updatePos( double dt )
{
	pos += vel * dt;

	if (abs(rotation) > 1e-8)
	{
		orientation += rotation * dt;	
	}	
}

void RigidBody::updateVel( double dt )
{
	if (mass < 1e8)
	{
		vel += dt * netForce / mass;
		rotation += dt * netTorque / moment;
	}
}

void RigidBody::applyForce( Vec2d x, Vec2d f )
{
	if (mass > 1e8)
	{
		return;
	}

	if (!(-1e8 < f[0] && f[0] < 1e8 && -1e8 < f[1] && f[1] < 1e8))
	{
		printf("what the fuck impulse\n");
		return;
	}

	rebuildMatrices();

	netForce += f;

	Vec3d Rx = Vec3d(x[0], x[1], 0) - Vec3d(pos[0], pos[1], 0);
	Vec3d ff(f[0], f[1], 0);
	Vec3d torque = cross(Rx, ff);

	netTorque += torque[2];
}

void RigidBody::resetForce()
{
	netForce = Vec2d(0, 0);
	netTorque = 0;
}
